#include "Foc_math.h"
#include "cordic.h"
#include "d_math.h"

#define TS (0.001f)

#define N_BASE    6.0f             // 3S电池
#define BATVEL    (4.0f * N_BASE)  // 电池电压
#define INVBATVEL (1.0f / BATVEL)  // 电池电压的倒数

/**
 * @brief 计算正弦和余弦值
 *
 * 计算电角度 theta 对应的 sin 和 cos 值
 *
 * @param foc 指向电机数据结构的指针
 */
Circular_t DAP_Sin_Cos(float ElAngle)
{
    Circular_t Circular;
    Circular.sin_val = arm_sin_f32(ElAngle);  // 计算正弦值
    Circular.cos_val = arm_cos_f32(ElAngle);  // 计算余弦值
    return Circular;
}

Circular_t CORDIC_sin_cos(float ElAngle)
{
    Circular_t Circular;
    float Angle0 = ElAngle / M_2PI;
    uint32_t Angle1 = (uint32_t)(Angle0 * 2147483648.0f);
    uint32_t Angle2;
    CORDIC->WDATA = Angle1;
    while(HAL_CORDIC_GetState(&hcordic) != HAL_CORDIC_STATE_READY)
    {
        Angle2 = (Angle1 + 0x20000000) & 0x80000000;
    }
    CORDIC->WDATA = Angle2;
    while(HAL_CORDIC_GetState(&hcordic) != HAL_CORDIC_STATE_READY)
    {
        float Tmp1 = CORDIC->RDATA;
        Circular.sin_val = Tmp1 / 2147483648.0f * M_2PI;
    }
    float Tmp2 = CORDIC->RDATA;
    Circular.cos_val = Tmp2 / 2147483648.0f * M_2PI;
    return Circular;
}

/**
 * @brief Clarke变换
 *
 * Clarke变换将三相电流转换为 Alpha-Beta 坐标系下的电流
 *
 * @param foc 指向电机数据结构的指针
 */
I_alphabeta_t Clarke(I_adc_t* I_adc)
{
    I_alphabeta_t I_alphabeta;
    I_alphabeta.i_alpha = I_adc->i_a;                               // α轴电流等于a相电流
    I_alphabeta.i_beta = (I_adc->i_b - I_adc->i_c) * ONE_BY_SQRT3;  // β轴电流计算，使用√3的倒数进行归一化
    return I_alphabeta;
}

/**
 * @brief 逆Clarke变换
 *
 * 逆Clarke变换将 Alpha-Beta 坐标系下的电压转换为三相电压
 *
 * @param foc 指向电机数据结构的指针
 */
V_adc_t Inv_clarke(V_alphabeta_t* V_alphabeta)
{
    V_adc_t V_adc;
    V_adc.v_a = V_alphabeta->v_alpha;                                               // a相电压等于α轴电压
    V_adc.v_b = -0.5f * V_alphabeta->v_alpha + TWO_BY_SQRT3 * V_alphabeta->v_beta;  // b相电压计算
    V_adc.v_c = -0.5f * V_alphabeta->v_alpha - TWO_BY_SQRT3 * V_alphabeta->v_beta;  // c相电压计算
    return V_adc;
}

/**
 * @brief Park变换
 *
 * Park变换将 Alpha-Beta 坐标系下的电流转换为 dq 坐标系下的电流，转换成了两个恒定的直流分量
 *
 * @param foc 指向电机数据结构的指针
 */
I_dq_t Park(I_alphabeta_t* I_alphabeta, Circular_t* Circular)
{
    I_dq_t I_dq;
    I_dq.i_d = I_alphabeta->i_alpha * Circular->cos_val + I_alphabeta->i_beta * Circular->sin_val;  // d轴电流计算
    I_dq.i_q = I_alphabeta->i_beta * Circular->cos_val - I_alphabeta->i_alpha * Circular->sin_val;  // q轴电流计算
    return I_dq;
}

/**
 * @brief 逆Park变换
 *
 * 反Park变换将 dq 坐标系下的电压转换为 Alpha-Beta 坐标系下的电压
 *
 * @param foc 指向电机数据结构的指针
 */
V_alphabeta_t Inv_Park(V_dq_t* V_dq, Circular_t* Circular)
{
    V_alphabeta_t V_alphabeta;
    V_alphabeta.v_alpha = (V_dq->v_d * Circular->cos_val - V_dq->v_q * Circular->sin_val);  // α轴电压计算
    V_alphabeta.v_beta = (V_dq->v_d * Circular->sin_val + V_dq->v_q * Circular->cos_val);   // β轴电压计算
    return V_alphabeta;
}

/**
 * @brief 均值零序分量注入
 *
 * 对α-β坐标系的电压进行均值零序分量注入，以便进行SVPWM调制
 * 电压角度是垂直于转子角度的，n代表的是合成电压的扇区，不是转子所在的扇区
 *
 * @param foc 指向电机数据结构的指针
 */
V_adc_t Svpwm_Midpoint(V_alphabeta_t* V_alphabeta)
{
    V_adc_t V_adc;
    // 根据母线电压调整α和β轴电压
    V_alphabeta->v_alpha = INVBATVEL * V_alphabeta->v_alpha;
    V_alphabeta->v_beta = INVBATVEL * V_alphabeta->v_beta;

    float va = V_alphabeta->v_alpha;                                               // 保存α轴电压
    float vb = -0.5f * V_alphabeta->v_alpha + TWO_BY_SQRT3 * V_alphabeta->v_beta;  // 计算b相电压
    float vc = -0.5f * V_alphabeta->v_alpha - TWO_BY_SQRT3 * V_alphabeta->v_beta;  // 计算c相电压

    float vmax = fmaxf(fmaxf(va, vb), vc);  // 找到最大电压值
    float vmin = fminf(fminf(va, vb), vc);  // 找到最小电压值

    float vcom = (vmax + vmin) * 0.5f;  // 计算零序分量

    // 计算各相的占空比调整值，确保在0到1之间
    V_adc.v_a = fminf(fmaxf((vcom - va) + 0.5f, 0.0f), 1.0f);
    V_adc.v_b = fminf(fmaxf((vcom - vb) + 0.5f, 0.0f), 1.0f);
    V_adc.v_c = fminf(fmaxf((vcom - vc) + 0.5f, 0.0f), 1.0f);

    return V_adc;
}

/**
 * @brief 扇区判断
 *
 * 根据α-β坐标系中的电压值判断当前所在扇区，并计算各相的占空比
 *
 * @param foc 指向电机数据结构的指针
 */
V_adc_t Svpwm_Sector(V_alphabeta_t* V_alphabeta)
{
    V_adc_t V_adc;
    float ta = 0.0f, tb = 0.0f, tc = 0.0f;                                    // 各相占空比时间初始化
    float k = (TS * SQRT3) * INVBATVEL;                                       // 根据母线电压计算k值
    float va = V_alphabeta->v_beta;                                           // β轴电压
    float vb = (SQRT3 * V_alphabeta->v_alpha - V_alphabeta->v_beta) * 0.5f;   // b相电压计算
    float vc = (-SQRT3 * V_alphabeta->v_alpha - V_alphabeta->v_beta) * 0.5f;  // c相电压计算
    int a = (va > 0.0f) ? 1 : 0;            // 判断a相是否大于零，结果为1或0
    int b = (vb > 0.0f) ? 1 : 0;            // 判断b相是否大于零，结果为1或0
    int c = (vc > 0.0f) ? 1 : 0;            // 判断c相是否大于零，结果为1或0
    int sextant = (c << 2) + (b << 1) + a;  // 根据a、b、c相的状态确定扇区

    switch(sextant)
    {
        case SECTOR_3:  // 扇区3
        {
            float t4 = k * vb;
            float t6 = k * va;
            float t0 = (TS - t4 - t6) * 0.5f;  // 零矢量的作用时间

            ta = t4 + t6 + t0;  // 计算a相占空比时间
            tb = t6 + t0;       // 计算b相占空比时间
            tc = t0;            // c相占空比时间为t0
        }
        break;

        case SECTOR_1:  // 扇区1
        {
            float t6 = -k * vc;
            float t2 = -k * vb;
            float t0 = (TS - t2 - t6) * 0.5f;

            ta = t6 + t0;       // a相占空比时间计算
            tb = t2 + t6 + t0;  // b相占空比时间计算
            tc = t0;            // c相占空比时间为t0
        }
        break;

        case SECTOR_5:  // 扇区5
        {
            float t2 = k * va;
            float t3 = k * vc;
            float t0 = (TS - t2 - t3) * 0.5f;

            ta = t0;            // a相占空比时间为t0
            tb = t2 + t3 + t0;  // b相占空比时间计算
            tc = t3 + t0;       // c相占空比时间计算
        }
        break;

        case SECTOR_4:  // 扇区4
        {
            float t1 = -k * va;
            float t3 = -k * vb;
            float t0 = (TS - t1 - t3) * 0.5f;

            ta = t0;            // a相占空比时间为t0
            tb = t3 + t0;       // b相占空比时间计算
            tc = t1 + t3 + t0;  // c相占空比时间计算
        }
        break;

        case SECTOR_6:  // 扇区6
        {
            float t1 = k * vc;
            float t5 = k * vb;
            float t0 = (TS - t1 - t5) * 0.5f;

            ta = t5 + t0;       // a相占空比时间计算
            tb = t0;            // b相占空比时间为t0
            tc = t1 + t5 + t0;  // c相占空比时间计算
        }
        break;

        case SECTOR_2:  // 扇区2
        {
            float t4 = -k * vc;
            float t5 = -k * va;
            float t0 = (TS - t4 - t5) * 0.5f;

            ta = t4 + t5 + t0;  // a相占空比时间计算
            tb = t0;            // b相占空比时间为t0
            tc = t5 + t0;       // c相占空比时间计算
        }
        break;

        default:
            break;  // 默认情况，不做处理
    }

    // 更新各相地占空比调整值，反转以适应SVPWM调制方式，使其在[0,1]范围内。
    V_adc.v_a = fminf(fmaxf(1.0f - ta, 0.0f), 1.0f);
    V_adc.v_b = fminf(fmaxf(1.0f - tb, 0.0f), 1.0f);
    V_adc.v_c = fminf(fmaxf(1.0f - tc, 0.0f), 1.0f);

    return V_adc;
}
